//
// Created by Administrator on 2021/9/28.
//

#include "Matrix4x4.h"


Matrix4x4::Matrix4x4() : MatrixN(4,4) {
}

Matrix4x4 Matrix4x4::operator*(const Matrix4x4 &m) const {
    return Matrix4x4::multiply(*this, m);
}


Matrix4x4 Matrix4x4::operator*(const float f) const {
    Matrix4x4 r;
    r.data = {
            this->data[0] * f, this->data[1] * f, this->data[2] * f, this->data[3] * f,
            this->data[4] * f, this->data[5] * f, this->data[6] * f, this->data[7] * f,
            this->data[8] * f, this->data[9] * f, this->data[10] * f, this->data[11] * f,
            this->data[12] * f, this->data[13] * f, this->data[14] * f, this->data[15] * f,
    };
    return r;
}

/**
 * 乘法
 * @param m1 矩阵1
 * @param m2 矩阵2
 * @return
 */
Matrix4x4 Matrix4x4::multiply(const Matrix4x4& m1, const Matrix4x4& m2) {
    MAssert::check(m1.col == m2.row, "矩阵行->列不一致,无法相乘");

    Matrix4x4 r;
    for(int i = 0; i < m1.row; i++){
        for(int k = 0; k < m1.col; k++){
            float sum = 0.0f;
            for(int j = 0; j < m2.row; j++){
                float m1r = m1.data[i * m1.row + j];
                float m2c = m2.data[j * m2.row + k];
                sum += m1r * m2c;
            }
            r.data[i * m1.row + k] = sum;
        }
    }

    //log_info("multiply r:%s", r.toString().c_str());
    return r;
}

/**
 * 转置 行转列  列转行
 * @return
 */
Matrix4x4 Matrix4x4::transpose() {
    float m11 = data[0], m12 = data[1], m13 = data[2], m14 = data[3];
    float m21 = data[4], m22 = data[5], m23 = data[6], m24 = data[7];
    float m31 = data[8], m32 = data[9], m33 = data[10], m34 = data[11];
    float m41 = data[12], m42 = data[13], m43 = data[14], m44 = data[15];

    Matrix4x4 r;
    r.data = {
            m11, m21, m31, m41,
            m12, m22, m32, m42,
            m13, m23, m33, m43,
            m14, m24, m34, m44,
    };
    return r;
}

/**
 * 行列式 TODO 未验证！
 *
 * detM = m11[m22(m33*m44-m34*m43) + m23(m34*m42-m32*m44) + m24(m32*m43-m33*m42)]
 *       -m12[m21(m33*m44-m34*m43) + m23(m34*m41-m31*m44) + m24(m31*m43-m33*m41)]
 *       +m13[m21(m32*m44-m34*m42) + m22(m34*m41-m31*m44) + m24(m31*m42-m32*m41)]
 *       -m14[m21(m32*m43-m33*m42) + m22(m33*m41-m31*m43) + m23(m31*m42-m32*m41)]
 * @return
 */
float Matrix4x4::determinant() {
    float m11 = data[0], m12 = data[1], m13 = data[2], m14 = data[3];
    float m21 = data[4], m22 = data[5], m23 = data[6], m24 = data[7];
    float m31 = data[8], m32 = data[9], m33 = data[10], m34 = data[11];
    float m41 = data[12], m42 = data[13], m43 = data[14], m44 = data[15];

    return m11*(m22*(m33*m44-m34*m43) + m23*(m34*m42-m32*m44) + m24*(m32*m43-m33*m42))
          -m12*(m21*(m33*m44-m34*m43) + m23*(m34*m41-m31*m44) + m24*(m31*m43-m33*m41))
          +m13*(m21*(m32*m44-m34*m42) + m22*(m34*m41-m31*m44) + m24*(m31*m42-m32*m41))
          -m14*(m21*(m32*m43-m33*m42) + m22*(m33*m41-m31*m43) + m23*(m31*m42-m32*m41));
}

/**
 * 余子式  TODO 未验证！
 * +m11的子矩阵行列式 -m12的子矩阵行列式 ...
 * @return
 */
Matrix4x4 Matrix4x4::cofactors() {
    float m11 = data[0], m12 = data[1], m13 = data[2], m14 = data[3];
    float m21 = data[4], m22 = data[5], m23 = data[6], m24 = data[7];
    float m31 = data[8], m32 = data[9], m33 = data[10], m34 = data[11];
    float m41 = data[12], m42 = data[13], m43 = data[14], m44 = data[15];

    Matrix4x4 r;
    r.data = {
            +(m22*(m33*m44-m34*m43) + m23*(m34*m42-m32*m44) + m24*(m32*m43-m33*m42)),
            -(m21*(m33*m44-m34*m43) + m23*(m34*m41-m31*m44) + m24*(m31*m43-m33*m41)),
            +(m21*(m32*m44-m34*m42) + m22*(m34*m41-m31*m44) + m24*(m31*m42-m32*m41)),
            -(m21*(m32*m43-m33*m42) + m22*(m33*m41-m31*m43) + m23*(m31*m42-m32*m41)),
            +1,
            -1,
            +1,
            -1,
            +1,
            -1,
            +1,
            -1,
            +1,
            -1,
            +1,
            -1
    };
    return r;
}

/**
 * 求矩阵的逆 TODO 抄的
 *
 * M^-1 = Madj / |M|  --> M的经典伴随矩阵/矩阵的行列式
 * 经典伴随矩阵 = 矩阵的余子式的转置
 *
 * @return 逆矩阵
 */
Matrix4x4 Matrix4x4::inverse() {
    float m00 = data[0 * 4 + 0];
    float m01 = data[0 * 4 + 1];
    float m02 = data[0 * 4 + 2];
    float m03 = data[0 * 4 + 3];
    float m10 = data[1 * 4 + 0];
    float m11 = data[1 * 4 + 1];
    float m12 = data[1 * 4 + 2];
    float m13 = data[1 * 4 + 3];
    float m20 = data[2 * 4 + 0];
    float m21 = data[2 * 4 + 1];
    float m22 = data[2 * 4 + 2];
    float m23 = data[2 * 4 + 3];
    float m30 = data[3 * 4 + 0];
    float m31 = data[3 * 4 + 1];
    float m32 = data[3 * 4 + 2];
    float m33 = data[3 * 4 + 3];
    float tmp_0  = m22 * m33;
    float tmp_1  = m32 * m23;
    float tmp_2  = m12 * m33;
    float tmp_3  = m32 * m13;
    float tmp_4  = m12 * m23;
    float tmp_5  = m22 * m13;
    float tmp_6  = m02 * m33;
    float tmp_7  = m32 * m03;
    float tmp_8  = m02 * m23;
    float tmp_9  = m22 * m03;
    float tmp_10 = m02 * m13;
    float tmp_11 = m12 * m03;
    float tmp_12 = m20 * m31;
    float tmp_13 = m30 * m21;
    float tmp_14 = m10 * m31;
    float tmp_15 = m30 * m11;
    float tmp_16 = m10 * m21;
    float tmp_17 = m20 * m11;
    float tmp_18 = m00 * m31;
    float tmp_19 = m30 * m01;
    float tmp_20 = m00 * m21;
    float tmp_21 = m20 * m01;
    float tmp_22 = m00 * m11;
    float tmp_23 = m10 * m01;

    float t0 = (tmp_0 * m11 + tmp_3 * m21 + tmp_4 * m31) -
             (tmp_1 * m11 + tmp_2 * m21 + tmp_5 * m31);
    float t1 = (tmp_1 * m01 + tmp_6 * m21 + tmp_9 * m31) -
             (tmp_0 * m01 + tmp_7 * m21 + tmp_8 * m31);
    float t2 = (tmp_2 * m01 + tmp_7 * m11 + tmp_10 * m31) -
             (tmp_3 * m01 + tmp_6 * m11 + tmp_11 * m31);
    float t3 = (tmp_5 * m01 + tmp_8 * m11 + tmp_11 * m21) -
             (tmp_4 * m01 + tmp_9 * m11 + tmp_10 * m21);

    float d = 1.0 / (m00 * t0 + m10 * t1 + m20 * t2 + m30 * t3); // 1/行列式

    Matrix4x4 r;
    r.data = {
            d * t0,  d * t1, d * t2, d * t3,
            d * ((tmp_1 * m10 + tmp_2 * m20 + tmp_5 * m30) -
                 (tmp_0 * m10 + tmp_3 * m20 + tmp_4 * m30)),
            d * ((tmp_0 * m00 + tmp_7 * m20 + tmp_8 * m30) -
                 (tmp_1 * m00 + tmp_6 * m20 + tmp_9 * m30)),
            d * ((tmp_3 * m00 + tmp_6 * m10 + tmp_11 * m30) -
                 (tmp_2 * m00 + tmp_7 * m10 + tmp_10 * m30)),
            d * ((tmp_4 * m00 + tmp_9 * m10 + tmp_10 * m20) -
                 (tmp_5 * m00 + tmp_8 * m10 + tmp_11 * m20)),
            d * ((tmp_12 * m13 + tmp_15 * m23 + tmp_16 * m33) -
                 (tmp_13 * m13 + tmp_14 * m23 + tmp_17 * m33)),
            d * ((tmp_13 * m03 + tmp_18 * m23 + tmp_21 * m33) -
                 (tmp_12 * m03 + tmp_19 * m23 + tmp_20 * m33)),
            d * ((tmp_14 * m03 + tmp_19 * m13 + tmp_22 * m33) -
                 (tmp_15 * m03 + tmp_18 * m13 + tmp_23 * m33)),
            d * ((tmp_17 * m03 + tmp_20 * m13 + tmp_23 * m23) -
                 (tmp_16 * m03 + tmp_21 * m13 + tmp_22 * m23)),
            d * ((tmp_14 * m22 + tmp_17 * m32 + tmp_13 * m12) -
                 (tmp_16 * m32 + tmp_12 * m12 + tmp_15 * m22)),
            d * ((tmp_20 * m32 + tmp_12 * m02 + tmp_19 * m22) -
                 (tmp_18 * m22 + tmp_21 * m32 + tmp_13 * m02)),
            d * ((tmp_18 * m12 + tmp_23 * m32 + tmp_15 * m02) -
                 (tmp_22 * m32 + tmp_14 * m02 + tmp_19 * m12)),
            d * ((tmp_22 * m22 + tmp_16 * m02 + tmp_21 * m12) -
                 (tmp_20 * m12 + tmp_23 * m22 + tmp_17 * m02))
    };

    return  r;
}

/**
 * 平移变换
 * @return 变换矩阵
 */
Matrix4x4 Matrix4x4::translation(const Matrix4x4& m, const Vector3& v) {
    Matrix4x4 t;
    t.data = {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        v.x, v.y, v.z, 1
    };
    return m * t;
}

/**
 * 旋转变换
 * @return 旋转矩阵
 */
Matrix4x4 Matrix4x4::rotation(const Matrix4x4& m, float rad, const Vector3& v) {
    if(rad == 0) return m;
    Matrix4x4 t;
    float c = cos(rad);
    float s = sin(rad);
    if(v.x > 0){
        t.data = {
            1, 0, 0, 0,
            0, c, s, 0,
            0, -s, c, 0,
            0, 0,  0, 1
        };
    }else if(v.y > 0){
        t.data = {
                c, 0, -s, 0,
                0, 1, 0, 0,
                -s, 0, c, 0,
                0, 0,  0, 1
        };
    }else if(v.z > 0){
        t.data = {
                c, s, 0, 0,
                -s, c, 0, 0,
                0, 0, 1, 0,
                0, 0,  0, 1
        };
    }else {
        throw MBaseException("只支持围绕单一轴旋转");
    }
    return m * t;
}

/**
 * 缩放变换
 * @return 缩放矩阵
 */
Matrix4x4 Matrix4x4::scale(const Matrix4x4& m, const Vector3& v) {
    Matrix4x4 t;
    t.data = {
        v.x, 0, 0, 0,
        0, v.y, 0, 0,
        0, 0, v.z, 0,
        0, 0,   0, 1,
    };
    return m * t;
}

/**
 * 正交投影
 * @param left 左
 * @param right 右
 * @param bottom 下
 * @param top 上
 * @param near 近
 * @param far 远
 * @return 投影矩阵
 *
 */
Matrix4x4 Matrix4x4::orthographic(float left, float right, float bottom, float top, float near, float far) {
    Matrix4x4 r;
    r.data = {
            2 / (right - left), 0, 0, 0,
            0, 2 / (top - bottom), 0, 0,
            0, 0, -2 / (far - near), 0,
            -(right + left) / (right - left), -(top + bottom) / (top - bottom), -(far + near) / (far - near), 1
    };
    return r;
}

/**
 * 透视矩阵
 * @param radfov 视野角度
 * @param aspect 宽高比
 * @param near 近
 * @param far 远
 * @return 投影矩阵
 */
Matrix4x4 Matrix4x4::perspective(float radfov, float aspect, float near, float far) {

    float fovtanHalf = tan(radfov / 2.0);
    float rangeInv = 1.0f / (near - far);

    Matrix4x4 r;
    r.data = {
        1 / (aspect * fovtanHalf), 0, 0, 0,
        0, 1 / fovtanHalf, 0, 0,
        0, 0, (near + far) * rangeInv, -1,
        0, 0, near * far * rangeInv * 2, 0,
    };
    return r;
}

/**
 * Lookat 相机矩阵
 *
 * 先平移到相机位置，再执行旋转
 *
 * @param position 位置
 * @param target 观察点
 * @param up 上方向
 * @return 矩阵
 */
Matrix4x4 Matrix4x4::lookAt(const Vector3& position, const Vector3& target, const Vector3& up) {
    //z方向
    Vector3 zDir = position - target;
    zDir.normalize();
    //x方向
    Vector3 xDir = Vector3::crossProduct(zDir, up);
    xDir.normalize();
    //y方向
    Vector3 yDir = Vector3::crossProduct(zDir, xDir);
    yDir.normalize();

    Matrix4x4 r;
    r.data = {
//            xDir.x, xDir.y, xDir.z, 0, 注意，这里y为什么反了？
//            yDir.x, yDir.y, yDir.z, 0,
//            zDir.x, zDir.y, zDir.z, 0,
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            position.x, position.y, position.z, 1,
    };
    return r;
}

/**
 * 向量相乘 TODO 有问题
 * @param v
 * @return
 */
Vector3 Matrix4x4::operator*(const Vector3 v) {
    MatrixN m(4, 4);
    MatrixN vc(4, 1);

    m.data = {
            data[0], data[1], data[2], data[3],
            data[4], data[5], data[6], data[7],
            data[8], data[9], data[10], data[11],
            data[12], data[13], data[14], data[15]
    };

    vc.data = {
            v.x,
            v.y,
            v.z,
            1.0f
    };

    MatrixN result = m * vc;

    Vector3 resultVec;
    resultVec.x = result.data[0];
    resultVec.y = result.data[1];
    resultVec.z = result.data[2];

    return resultVec;
}





